/***************************************************************************************************************************
 * Author: guanty18
 *
 * Update Time: 2021.4.9
***************************************************************************************************************************/
#include <fstream>
#include <math.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <sensor_msgs/Range.h>
#include <uav/servo.h>
#include <uav/position.h>

//*******************************全局变量*******************************




//参数
bool i_location;
bool i_payload;
bool i_avoid;
float alpha;
float takeoff_height;
float payload_height;
float max_error;
float camera_x;
float camera_y;
float servo_x[3];
float servo_y[3];
float error[3];


bool turn_offboard = true;
bool is_land=false;
int command_num=-1;
int find_num=0;

//舵机操控信息
uav::servo servo_info;
int servo_id=0;

//无人机飞行地图目标位置
geometry_msgs::PoseStamped pose;//无人机飞行实际目标位置
geometry_msgs::PoseStamped real_pose;
//pose.pose.position.x = 0;
//pose.pose.position.y = 0;
//pose.pose.position.z = 0;



//无人机状态变量及其订阅回调
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}


float tfmini_z;
void tfmini_cb(const sensor_msgs::Range::ConstPtr& msg)
{
    tfmini_z = msg->range;
}

// //无人机OFFBOARD模式
// void rc_cb(const mavros_msgs::RCIn::ConstPtr& rcmsg)
// {
// 	if (rcmsg->channels[7] > 1800) 
//     {
// 		turn_offboard = true;
// 	} 
//     else 
//     {
// 		turn_offboard = false;
// 	}
// }

//无人机实际位置变量及其订阅回调
int clock_flag=0;
bool is_arrived = false;
uav::position position_now;
void pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    double dx = msg->pose.position.x - pose.pose.position.x;
	double dy = msg->pose.position.y - pose.pose.position.y;
    double dz = msg->pose.position.z - pose.pose.position.z;
	if (abs(dx) < error[0] && abs(dy) < error[1] && abs(dz) < error[2]) {
        clock_flag++;
		if (!is_arrived && clock_flag>=20)
			is_arrived = true;
    }

    position_now.x = msg->pose.position.x;
    position_now.y = msg->pose.position.y;
    position_now.z = msg->pose.position.z;
}

//无人机摄像头目标变量及其订阅回调
float pos_error;
uav::position position_target;
void target_cb(const uav::position::ConstPtr& msg){
    
	float view_L = position_now.z * 0.577;
    float x_error = view_L*(msg->x) + camera_x + servo_x[servo_id];
    float y_error = view_L*(msg->y) + camera_y + servo_y[servo_id];

    pos_error=x_error*x_error+y_error*y_error;

    position_target.x=position_now.x-y_error;
    position_target.y=position_now.y-x_error;
    position_target.z=position_now.z;
}

//激光雷达最近点-障碍物
uav::position position_barrier_tmp;
uav::position position_barrier;
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg){
    float distance_min = msg->ranges[0];
    float angle = msg->angle_min;
    for(int i = 0; i < 360; i++) {
        if(msg->ranges[i] < distance_min){
            angle = (msg->angle_min + msg->angle_increment * i);
            distance_min = msg->ranges[i];
        }
    }
    position_barrier_tmp.x=distance_min*cos(angle);
    position_barrier_tmp.y=distance_min*sin(angle);
}



//函数封装
void fly_to_pose(ros::Publisher local_pos_pub, ros::Rate rate)
{
    while(ros::ok() && !is_arrived)
    {   
		local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
        ROS_INFO("point: %d, \ntarget.xyz: %0.5f/%0.5f/%0.5f, \npose.xyz: %0.5f/%0.5f/%0.5f", command_num,
		        pose.pose.position.x, pose.pose.position.y, pose.pose.position.z,
				position_now.x, position_now.y, position_now.z);

    }

    is_arrived=false;
    clock_flag=0;
}


//*******************************主函数********************************
int main(int argc, char **argv)
{
    ros::init(argc, argv, "task");
    ros::NodeHandle nh;


    
    //订阅和发布消息
    // ros::Subscriber rc_sub = nh.subscribe<mavros_msgs::RCIn>
    //         ("/mavros/rc/in", 50, rc_cb);
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("/mavros/state", 50, state_cb);
    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("/mavros/local_position/pose", 50, pos_cb);
    ros::Subscriber laser_sub = nh.subscribe<sensor_msgs::LaserScan>
            ("/laser/scan", 50, scanCallback);
    ros::Subscriber target_sub = nh.subscribe<uav::position>
            ("/uav/target", 50, target_cb);
	ros::Subscriber tfmini_sub = nh.subscribe<sensor_msgs::Range>
            ("/mavros/distance_sensor/hrlv_ez4_pub", 50, tfmini_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("/mavros/setpoint_position/local", 20);
    ros::Publisher cmd_vel_pub = nh.advertise<geometry_msgs::Twist>
            ("/mavros/setpoint_velocity/cmd_vel_unstamped", 20);
    ros::Publisher servo_pub = nh.advertise<uav::servo>
            ("/uav/servo", 20);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("/mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("/mavros/set_mode");

    // 参数
    float Ax = 0.0;
    float Ay = 2.0;
    float Bx = 2.0;
    float By = 0.0;
    float Cx = 2.0;
    float Cy = 2.0;
    nh.param<bool>("i_location", i_location, true);
    nh.param<bool>("i_payload", i_payload, true);    
    nh.param<bool>("i_avoid", i_avoid, true);
    nh.param<float>("alpha", alpha, 0.0);
    nh.param<float>("takeoff_height", takeoff_height, 1.0);
    nh.param<float>("payload_height", payload_height, 0.2);
    nh.param<float>("errorx", error[0], 0.05);
    nh.param<float>("errory", error[1], 0.05);
    nh.param<float>("errorz", error[2], 0.05);
    nh.param<float>("camera_x", camera_x, 0.0);
    nh.param<float>("camera_y", camera_y, 0.0);
    nh.param<float>("servo1x", servo_x[0], 0.0);
    nh.param<float>("servo1y", servo_y[0], 0.0);    
    nh.param<float>("servo2x", servo_x[1], 0.0);
    nh.param<float>("servo2y", servo_y[1], 0.0);    
    nh.param<float>("servo3x", servo_x[2], 0.0);
    nh.param<float>("servo3y", servo_y[2], 0.0);




	ros::Rate rate(20);
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::Twist cmd_vel;
    cmd_vel.linear.x = 0.1;
    cmd_vel.linear.y = 0.1;
    cmd_vel.linear.z = 0.1;

    cmd_vel.angular.x = 0;
    cmd_vel.angular.y = 0;
    cmd_vel.angular.z = 0;
    cmd_vel_pub.publish(cmd_vel);

    

	mavros_msgs::SetMode set_mode;
	mavros_msgs::CommandBool arm_cmd;
	set_mode.request.custom_mode = "OFFBOARD";
	arm_cmd.request.value = true;
	
	pose.pose.position.x = 0;
    pose.pose.position.y = 0;
	pose.pose.position.z = 0;
    ros::Time last_request = ros::Time::now();

    ROS_INFO("success");

    while(ros::ok()){
		if (turn_offboard && !is_land)
        {   
            while( !current_state.armed && ((ros::Time::now() - last_request) > ros::Duration(0.2)))
            {
                if( arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }

			while( current_state.mode != "OFFBOARD" && ((ros::Time::now() - last_request) > ros::Duration(0.05)))
            {   
                set_mode.request.custom_mode = "OFFBOARD";
                local_pos_pub.publish(pose);
				if( set_mode_client.call(set_mode) && set_mode.response.mode_sent)
                {
					ROS_INFO("Offboard enabled");
				}
                last_request = ros::Time::now();
            }
            

        }

        if( current_state.armed )
        {

        switch (command_num) 
        {
			case -1:
				if (current_state.mode == "OFFBOARD" && current_state.armed){
					local_pos_pub.publish(pose);
					command_num++;
				}
				break;
			case 0:
                //takeoff
                pose.pose.position.x = 0.0;
                pose.pose.position.y = 0.0;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "take off!"<<std::endl;
                command_num++;
                break;

            case 1:
                // from O to A (nearly)
                pose.pose.position.x = Ax;
                pose.pose.position.y = Ay;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "arrived at A"<<std::endl;
                command_num++;
                break;

            case 2:
                // find A (excatly)
                while(pos_error==0)
                {
                    pose.pose.position.x = position_now.x;
                    pose.pose.position.y = position_now.y;
                    pose.pose.position.z = position_now.z;
                    fly_to_pose(local_pos_pub, rate);
                    std::cout << "find A ..."<<find_num<<std::endl;

                    ros::spinOnce();       
                    rate.sleep();                 
                }

                error[0]=0.02;
                error[1]=0.02;

                pose.pose.position.x = position_target.x;
                pose.pose.position.y = position_target.y;
                pose.pose.position.z = payload_height;
                fly_to_pose(local_pos_pub, rate);

                set_mode.request.custom_mode = "AUTO.LAND";
                while( current_state.mode != "AUTO.LAND" && ((ros::Time::now() - last_request) > ros::Duration(0.2)))
                {
                    if( set_mode_client.call(set_mode) && set_mode.response.mode_sent )
                    {
                        ROS_INFO("AUTO.LAND");
                    }
                    last_request = ros::Time::now();
                }
                is_land = true;

                std::cout << "pre to payload at A"<<std::endl;
                
                command_num++;
                break;

            case 3:
                // payload at A
                if(i_payload)
                {
                    servo_info.id = 33;
                    servo_info.direction = 1;
                    servo_pub.publish(servo_info);
                    std::cout << "payload at A!"<<std::endl;
                    servo_id++;

                    for(int clock=0;clock<=100;clock++)
                    {
                        rate.sleep();
                    }
                    is_land = false;

                    error[0]=0.05;
                    error[1]=0.05;


                }
                              
                command_num++;
                break;

            case 4:

                // re-up at A
                pose.pose.position.x = position_now.x; 
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "re-takeoff at A!"<<std::endl;

                // from A to barrier1
                pose.pose.position.x = position_now.x+0.3;
                pose.pose.position.y = position_now.y-0.3;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "approach barrier 1 !"<<std::endl;

                command_num++;
                break;

            case 5:
                // collision avoidance 1 
                if(i_avoid)
                {   
                    position_barrier = position_barrier_tmp;
                    std::cout << "x:"<<position_barrier.x<< "y:"<<position_barrier.y <<std::endl;

                    pose.pose.position.x = position_now.x;
                    pose.pose.position.y = position_now.y;
                    pose.pose.position.z = takeoff_height + 2.0;
                    fly_to_pose(local_pos_pub, rate);

                    pose.pose.position.x = position_now.x+position_barrier.x*2;
                    pose.pose.position.y = position_now.y+position_barrier.y*2;
                    pose.pose.position.z = takeoff_height + 2.0;
                    fly_to_pose(local_pos_pub, rate);
                    
                    pose.pose.position.x = position_now.x;
                    pose.pose.position.y = position_now.y;
                    pose.pose.position.z = takeoff_height;    
                    fly_to_pose(local_pos_pub, rate);
                    std::cout << "collision avoidance 1 !"<<std::endl;
                }
                command_num++;
                break;

            case 6:
                // from barrier1 to B
                pose.pose.position.x = Bx;
                pose.pose.position.y = By;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "arrived at B"<<std::endl;
                command_num++;
                break;

            case 7:
                // find B (excatly)
                while(pos_error==0)
                {
                    pose.pose.position.x = position_now.x;
                    pose.pose.position.y = position_now.y;
                    pose.pose.position.z = position_now.z;
                    fly_to_pose(local_pos_pub, rate);
                    std::cout << "find B ..."<<find_num<<std::endl;

                    ros::spinOnce();       
                    rate.sleep();                 
                }

                error[0]=0.02;
                error[1]=0.02;

                pose.pose.position.x = position_target.x;
                pose.pose.position.y = position_target.y;
                pose.pose.position.z = payload_height;
                fly_to_pose(local_pos_pub, rate);

                set_mode.request.custom_mode = "AUTO.LAND";
                while( current_state.mode != "AUTO.LAND" && ((ros::Time::now() - last_request) > ros::Duration(0.2)))
                {
                    if( set_mode_client.call(set_mode) && set_mode.response.mode_sent )
                    {
                        ROS_INFO("AUTO.LAND");
                    }
                    last_request = ros::Time::now();
                }
                is_land = true;

                std::cout << "pre to payload at B"<<std::endl;
                
                command_num++;
                break;

            case 8:
                // payload at B
                if(i_payload)
                {                 
                    servo_info.id = 35;
                    servo_info.direction = 1;
                    servo_pub.publish(servo_info);
                    std::cout << "payload at B!"<<std::endl;
                    servo_id++;

                    for(int clock=0;clock<=100;clock++)
                    {
                        rate.sleep();
                    }
                    is_land = false;

                    error[0]=0.05;
                    error[1]=0.05;


                }
                command_num++;
                break;

            case 9:
                // re-up at B
                pose.pose.position.x = position_now.x; 
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "re-takeoff at B!"<<std::endl;


                // collision avoidance 2 
                if(i_avoid)
                {
                    pose.pose.position.x = Bx;
                    pose.pose.position.y = By + 0.3;
                    pose.pose.position.z = takeoff_height;
                    fly_to_pose(local_pos_pub, rate);

                    position_barrier = position_barrier_tmp;

                    pose.pose.position.x = position_now.x;
                    pose.pose.position.y = position_now.y;
                    pose.pose.position.z = takeoff_height + 2.0;
                    fly_to_pose(local_pos_pub, rate);

                    pose.pose.position.x = position_now.x;
                    pose.pose.position.y = position_now.y+position_barrier.y*2;
                    pose.pose.position.z = takeoff_height + 2.0;
                    fly_to_pose(local_pos_pub, rate);
                    
                    pose.pose.position.x = position_now.x;
                    pose.pose.position.y = position_now.y;
                    pose.pose.position.z = takeoff_height;    
                    fly_to_pose(local_pos_pub, rate);
                    std::cout << "collision avoidance 2 !"<<std::endl;
                }
                command_num++;
                break;
            
            case 10:
                // from barrier2 to C
                pose.pose.position.x = Cx;
                pose.pose.position.y = Cy;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "arrived at C"<<std::endl;
                command_num++;
                break;

            case 11:
                // find C (excatly)
                while(pos_error==0)
                {
                    pose.pose.position.x = position_now.x;
                    pose.pose.position.y = position_now.y;
                    pose.pose.position.z = position_now.z;
                    fly_to_pose(local_pos_pub, rate);
                    std::cout << "find C ..."<<find_num<<std::endl;

                    ros::spinOnce();       
                    rate.sleep();                 
                }

                error[0]=0.02;
                error[1]=0.02;

                pose.pose.position.x = position_target.x;
                pose.pose.position.y = position_target.y;
                pose.pose.position.z = payload_height;
                fly_to_pose(local_pos_pub, rate);

                set_mode.request.custom_mode = "AUTO.LAND";
                while( current_state.mode != "AUTO.LAND" && ((ros::Time::now() - last_request) > ros::Duration(0.2)))
                {
                    if( set_mode_client.call(set_mode) && set_mode.response.mode_sent )
                    {
                        ROS_INFO("AUTO.LAND");
                    }
                    last_request = ros::Time::now();
                }
                is_land = true;

                std::cout << "pre to payload at C"<<std::endl;
                
                command_num++;
                break;

            case 12:
                // payload at C
                if(i_payload)
                {   
                    servo_info.id = 37;
                    servo_info.direction = 1;
                    servo_pub.publish(servo_info);
                    std::cout << "payload at C!"<<std::endl;
                    servo_id++;


                    for(int clock=0;clock<=100;clock++)
                    {
                        rate.sleep();
                    }
                    is_land = false;

                    error[0]=0.05;
                    error[1]=0.05;


                }
                command_num++;
                break;

            case 13:
                // re-up at C
                pose.pose.position.x = position_now.x; 
                pose.pose.position.y = position_now.y;
                pose.pose.position.z = takeoff_height;
                fly_to_pose(local_pos_pub, rate);
                std::cout << "re-takeoff at C!"<<std::endl;


                // back to O
                pose.pose.position.x = Cx;
                pose.pose.position.y = Cy;
                pose.pose.position.z = takeoff_height+2.0;
                fly_to_pose(local_pos_pub, rate);

                pose.pose.position.x = Cx/2;
                pose.pose.position.y = Cy/2;
                pose.pose.position.z = takeoff_height+2.0;
                fly_to_pose(local_pos_pub, rate);

                pose.pose.position.x = 0.0;
                pose.pose.position.y = 0.0;
                pose.pose.position.z = takeoff_height+2.0;
                fly_to_pose(local_pos_pub, rate);
                command_num++;
                break;

            case 14:    
                // find O (excatly)
                if(i_location)
                {
                    while(pos_error==0)
                    {
                        pose.pose.position.x = position_now.x;
                        pose.pose.position.y = position_now.y;
                        pose.pose.position.z = position_now.z;
                        fly_to_pose(local_pos_pub, rate);
                        std::cout << "find O ..."<<find_num<<std::endl;

                        ros::spinOnce();       
                        rate.sleep();                 
                    }


                    pose.pose.position.x = position_target.x;
                    pose.pose.position.y = position_target.y;
                    pose.pose.position.z = payload_height;
                    fly_to_pose(local_pos_pub, rate);
                }
                command_num++;
                break;

			
			case 15:
                set_mode.request.custom_mode = "AUTO.LAND";
                last_request = ros::Time::now();
                while( current_state.mode != "AUTO.LAND" && ((ros::Time::now() - last_request) > ros::Duration(2.0)))
                {
                     if( set_mode_client.call(set_mode) && set_mode.response.mode_sent)
                     {
                         std::cout << "AUTO.land"<<std::endl;
                     }
                }
                is_land=true;
               // 上锁
                last_request = ros::Time::now();
                arm_cmd.request.value = false;
                if( current_state.armed)
                {
                    if( arming_client.call(arm_cmd) &&arm_cmd.response.success)
                     {
                         std::cout << "Vehicle disarmed"<<std::endl;
                     }
                     last_request = ros::Time::now();
                 }
                break;
        }

        }
        

        ros::spinOnce();
        rate.sleep();
    }
    return 0;

}


